#ifndef GAZEBO_ROS_MOBROB_H
#define GAZEBO_ROS_MOBROB_H

#include <gazebo/Controller.hh>
#include <gazebo/Model.hh>
#include <gazebo/Geom.hh>
#include <gazebo/Time.hh>

#include <nav_msgs/Odometry.h>
#include <geometry_msgs/TwistWithCovariance.h>
#include <geometry_msgs/PoseWithCovariance.h>

#include <tf/transform_broadcaster.h>
#include <ros/ros.h>

#include <math.h>

namespace gazebo
{
  class GazeboRosMobrob : public Controller
  {
    public:
      GazeboRosMobrob( gazebo::Entity *parent );
      virtual ~GazeboRosMobrob();

      virtual void LoadChild( XMLConfigNode *node );
      virtual void InitChild();
      virtual void FiniChild();
      virtual void UpdateChild();

    private:

      void OnCmdVel( const geometry_msgs::TwistConstPtr &msg);

      ros::NodeHandle *rosnode_;

      ros::Publisher odom_pub_;
      ros::Publisher ICR_pub_;
      ros::Publisher joint_state_pub_;

      ros::Subscriber cmd_vel_sub_;

      /// Names of all joints

      ParamT<std::string> *node_namespaceP_;
      ParamT<std::string> *wheel_rf_rot_z_joint_nameP_;
      ParamT<std::string> *wheel_lb_rot_z_joint_nameP_;
      ParamT<std::string> *wheel_lf_rot_z_joint_nameP_;
      ParamT<std::string> *wheel_rb_rot_z_joint_nameP_;
      ParamT<std::string> *wheel_rf_rot_y_joint_nameP_;
      ParamT<std::string> *wheel_lb_rot_y_joint_nameP_;
      ParamT<std::string> *wheel_lf_rot_y_joint_nameP_;
      ParamT<std::string> *wheel_rb_rot_y_joint_nameP_;
      ParamT<std::string> *base_arduino_platform_joint_nameP_;

      /// Coordinates of the wheels, offset from the middle of the mobrob (base_link)
      ParamT<float> *wheel_RF_offxP_;
      ParamT<float> *wheel_RF_offyP_;
      ParamT<float> *wheel_LB_offxP_;
      ParamT<float> *wheel_LB_offyP_;
      ParamT<float> *wheel_LF_offxP_;
      ParamT<float> *wheel_LF_offyP_;
      ParamT<float> *wheel_RB_offxP_;
      ParamT<float> *wheel_RB_offyP_;

      /// Diameter of the big wheels
      ParamT<float> *wheel_big_diamP_;

      /// Diameter of the small wheels
      ParamT<float> *wheel_small_diamP_;

      ///Torque applied to the wheels
      ParamT<float> *torqueP_Y_;
      ParamT<float> *torqueP_Z_;

      Model *my_parent_;

      /// Speeds of the wheels
      float *wheel_speed_;
      /// Position of the wheels
      float *wheel_pos_;

      /// Simulation time of the last update
      Time prev_update_time_;
      Time last_cmd_vel_time_;

      /// Speed of the base_link

      double base_xB_p, base_yB_p, base_qB_p;

      /// Commanded speed of the base link

      double xB_p, yB_p, qB_p;

      /// odometry

      double odom_pose_[3];
      double odom_vel_[3];

      /// joint states

      bool set_joints_[9];
      Joint *joints_[9];


      tf::TransformBroadcaster transform_broadcaster_;
      sensor_msgs::JointState js_;

      void spin();
      boost::thread *spinner_thread_;
  };
}
#endif
